/*
 * MOTOR.c
 *
 * Created: 18-06-2013 23:31:42
 *  Author: Karmu
 */ 

#include <avr/io.h>
#include "MOTOR.h"

/*************************************************************************
Initialize motor
Input:    none
Returns:  none
*************************************************************************/
void motor_init()
{
		/* setup input/outputs */
		M_L_DDR |= _BV(M_L_IN1) | _BV(M_L_IN2) ;
		M_R_DDR |= _BV(M_R_IN1) | _BV(M_R_IN2) ;
		M_PWM_DDR |= _BV(M_L_PWM) | _BV(M_R_PWM);
		
		/*initialize pid switches */
		pidSwitchLeft=0;
		pidSwitchRight=0;
}

/*************************************************************************
Initialize OC1 A and B PWM
Input:	top \b ICR1 value to get phase and frequency PWM \n
		bottom \b OC1A and OC1B initial value to desired duty cycle\n
Returns:  none
*************************************************************************/
void motor_pwm_timer1_init(uint16_t top, uint16_t bottom_right, uint16_t bottom_left)
{
	/* setup phase corrected PWM for motor*/
	/* Set OCR1A/OCR1B on Compare Match when up-counting.
	Clear OCR1A/OCR1B on Compare Match when down-counting. */
	TCCR1A |= _BV(COM1A1) | _BV(COM1B1); // | _BV(COM1A0) | _BV(COM1B0);
	
	/* Phase Correct and Phase and Frequency Correct PWM */
	TCCR1B |= _BV(WGM13);
	
	/* Pre-scalar 1 */
	TCCR1B |= _BV(CS10);
	
	/* set the TOP value ICR1 */
	ICR1 = top;
	
	/* Set OCR1A and OCR1B to desired Duty cycle */
	OCR1A = bottom_left; //left_motor		5300
	OCR1B = bottom_right; //right_motor		3000
}

/*************************************************************************
Change PWM for right motor to change its speed
Input:	speed \b speed scale from 1 to 100\n
Returns:  none
*************************************************************************/
void motor_speed_right(uint8_t speed)
{
	if (speed>5 && speed<11)
	{
		/* Set OCR1B to desired Duty cycle */
		OCR1B = speed*1000;
	}		
}

/*************************************************************************
Change PWM for left motor to change its speed
Input:	speed \b speed scale from 1 to 100\n
Returns:  none
*************************************************************************/
void motor_speed_left(uint8_t speed)
{
	if (speed>5 && speed<11)
	{
		/* Set OCR1B to desired Duty cycle */
		OCR1A = speed*1000;
	}
}


/*************************************************************************
Set motor direction
Input:		motor		MOTOR_L or MOTOR_R
			direction	M_FORWARD, M_REVERSE or M_STOP
Returns:	none
*************************************************************************/
void set_motor_direction(uint8_t motor, uint8_t direction)
{
	if (motor==1)
	{
		switch (direction)
		{
			case M_FORWARD:
			M_L_PORT |=  _BV(M_L_IN2);		/* set DIR pin */
			M_L_PORT &= ~_BV(M_L_IN1);		/* clear DIR pin */
			pidSwitchLeft=1;
			break;
			case M_REVERSE:
			M_L_PORT &= ~_BV(M_L_IN2);		/* clear DIR pin */
			M_L_PORT |=  _BV(M_L_IN1);		/* set DIR pin */
			pidSwitchLeft=1;
			break;
			case M_STOP:
			M_L_PORT &= ~_BV(M_L_IN2);		/* clear DIR pin */
			M_L_PORT &= ~_BV(M_L_IN1);		/* clear DIR pin */
			pidSwitchLeft=0;
			/* Alternatively */
			//M_L_PORT |=  _BV(M_L_IN1);		/* set DIR pin */
			//M_L_PORT |=  _BV(M_L_IN2);		/* set DIR pin */
			break;
		}
	}
	else
	{
		switch (direction)
		{
			case M_FORWARD:
			M_R_PORT |=  _BV(M_R_IN2);		/* set DIR pin */
			M_R_PORT &= ~_BV(M_R_IN1);		/* clear DIR pin */
			pidSwitchRight=1;
			break;
			case M_REVERSE:
			M_R_PORT &= ~_BV(M_R_IN2);		/* clear DIR pin */
			M_R_PORT |=  _BV(M_R_IN1);		/* set DIR pin */
			pidSwitchRight=1;
			break;
			case M_STOP:
			M_R_PORT &= ~_BV(M_R_IN2);		/* clear DIR pin */
			M_R_PORT &= ~_BV(M_R_IN1);		/* clear DIR pin */
			pidSwitchRight=0;
			/* Alternatively */
			//M_R_PORT |=  _BV(M_R_IN1);		/* set DIR pin */
			//M_R_PORT |=  _BV(M_R_IN2);		/* set DIR pin */
			break;
		}
	}
}

/*************************************************************************
Steer the bot in a direction
Input:		direction	GO_LEFT, GO_HARD_LEFT, GO_FORWARD, GO_REVERSE,
						GO_HARD_RIGHT, GO_RIGHT, GO_BRAKE
Returns:	none
*************************************************************************/
uint8_t steer_robot(uint8_t direction)
{
	
	switch (direction)
	{
		case GO_LEFT:
		set_motor_direction(MOTOR_L, M_STOP);
		set_motor_direction(MOTOR_R, M_FORWARD);
		return GO_LEFT;
		
		case GO_HARD_LEFT:
		set_motor_direction(MOTOR_L, M_REVERSE);
		set_motor_direction(MOTOR_R, M_FORWARD);
		return GO_HARD_LEFT;
		
		case GO_FORWARD:
		set_motor_direction(MOTOR_L, M_FORWARD);
		set_motor_direction(MOTOR_R, M_FORWARD);
		return GO_FORWARD;
		
		case GO_REVERSE:
		set_motor_direction(MOTOR_L, M_REVERSE);
		set_motor_direction(MOTOR_R, M_REVERSE);
		return GO_REVERSE;
		
		case GO_HARD_RIGHT:
		set_motor_direction(MOTOR_L, M_FORWARD);
		set_motor_direction(MOTOR_R, M_REVERSE);
		return GO_HARD_RIGHT;
		
		case GO_RIGHT:
		set_motor_direction(MOTOR_L, M_FORWARD);
		set_motor_direction(MOTOR_R, M_STOP);
		return GO_RIGHT;
		
		case GO_BRAKE:
		set_motor_direction(MOTOR_L, M_STOP);
		set_motor_direction(MOTOR_R, M_STOP);
		return GO_BRAKE;
	}
	return 0;
}